/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include "CParticleFilter.h"

/**
* Update pose struct
* frames = num_frames*3 (num_frames = kaikkien pose pisteiden lkm)
 * @param  *P [frames x D] Internal coef - Harva kovarianssi... varaa tila
 * @param  *L [frames x D] - Harva cholesky esitys  - varaa tila
 * @param  *g [frames x 1] gradientti siirtymä - varaa muisti 
 * @param  *Xpos [frames x 1] Posteriori -       
 * @param  *Xpri [frames x 1] Priori - alkukäyrä
 * @param  *odo  [frames x 1] Differentiaalinen siirtymä vektori
 * @param  *base [N x 3] - Mittaukset poseista
 * @param  *frame [N * 1] - (int) indexit mittauksista
 * @param  *CC [frames x D] Internal coef - Harva kovarianssi... varaa tila - alusta nollaksi
 * @param  *Ce [Frames] Ristikovarianssi - alusta nollaksi
 * @param  *P0 [N x 3] Paino pose mittaukselle - suuri on iso paino eli niinku tosi hyvä mittaus
 * @param  *S  [3 x 1] Odometrian kohina (sama kaikille ny)
 * @param  *lamda [1x1] Levenberg marknad parametri - Alusta pieneksi  
 * @param  D Kovarianssimatriisin "bandwidth" - laita sellaseks neloseksi
 * @param  N Mittausten määrä
 * @param  num_frames kaikkien pose pisteiden lkm
 * @param  *er virhe ennen päivitttämistä (frames * frames)
 **/
struct UpdatePose{
		double *P;
		double *L;
		double *g;
		double *Xpos;
		double *Xpri;
		double *odo;
		double *base;
		int    *frame;
		double *CC;
		double *Ce;
		double *P0;
		double *S;
		double lambda;
		int D;
		int N;
		int num_frames;
		double *er;
		int frames;
								
		UpdatePose(){
			P=NULL;L=NULL;g=NULL;Xpos=NULL;Xpri=NULL;odo=NULL;
			base=NULL;frame=NULL;CC=NULL;Ce=NULL;P0=NULL;S=NULL;
			lambda = 0.00001;
			D=0;N=0;num_frames=0;er=NULL;	
		}
		~UpdatePose(){
				sfree(P);sfree(L);sfree(g);sfree(Xpos);sfree(Xpri);
				sfree(odo);sfree(base);sfree(frame);sfree(CC);sfree(Ce);
				sfree(P0);sfree(S);sfree(er);
		}
		void initialize(std::vector<mcl::pose> &odom, int _D){
				num_frames = odom.size();
				frames = num_frames*3;
				D=_D;
				P = (double *)malloc(sizeof(double)*frames*D);
				L = (double *)malloc(sizeof(double)*frames*D);
				g = (double *)malloc(sizeof(double)*frames);
				Xpos = (double *)malloc(sizeof(double)*frames);
				Xpri = (double *)malloc(sizeof(double)*frames);
				odo = (double *)malloc(sizeof(double)*frames);
				base = (double *)malloc(sizeof(double)*frames);
				frame =(int *) malloc(sizeof(int) * num_frames);
				CC = (double *) malloc(sizeof(double)*frames*D);
				Ce = (double *) malloc(sizeof(double)*frames);
				P0 = (double *) malloc(sizeof(double)*frames);
				S = (double *) malloc(3*sizeof(double));
				er = (double *) malloc(sizeof(double)*(frames+1)*(frames+1));
				S[0] = 1.0/(0.01*0.01);
				S[1] = 1.0/(0.01*0.01);
				S[2] = 1.0/((M_PI/180.0) * (2.0*M_PI/180.0));
				//S[2] = M_PI/180.0;
				mcl::pose diff;
				for(int i=0; i<odom.size()-1;i++){
						diff.setToDifferentialPose(odom[i+1],odom[i]);
						odo[3*i]=diff.x;
						odo[3*i+1]=diff.y;
						odo[3*i+2]=to2pi(diff.a);
				}
				memset(CC,0,sizeof(double)*frames*D);
				memset(Ce,0,sizeof(double)*frames);
				memset(Xpri,0,sizeof(double)*frames);
				memset(Xpos,0,sizeof(double)*frames);
		}
		
		void update(std::vector<mcl::pose> &meas, std::vector<int> &ind, std::vector<mcl::pose> &var){
				N = meas.size();
				memset(CC,0,sizeof(double)*frames*D);
				memset(Ce,0,sizeof(double)*frames);
				
				for(int i=0;i<N;i++){
						Xpri[3*i] = odo[3*i];
						Xpri[3*i+1] = odo[3*i+1];
						Xpri[3*i+2] = odo[3*i+2];
						
						base[3*i] = meas[i].x;
						base[3*i+1] = meas[i].y;
						base[3*i+2] = to2pi(meas[i].a);
						frame[i] = ind[i];
						P0[3*i] = .5/(var[i].x+.001);
						P0[3*i+1] = .5/(var[i].y+.001);
						//P0[3*i+2] = .5/(to2pi(var[i].a)+.001);
						P0[3*i+2] = M_PI/180.0;
				}
// 				//memcpy(Xpri,Xpos,sizeof(double)*frames);
				update_pose();
		}
		void iterate(){
				memset(CC,0,sizeof(double)*frames*D);
				memset(Ce,0,sizeof(double)*frames);
				memcpy(Xpri,Xpos,sizeof(double)*frames);
				for(int i=0;i<N;i++){
						Xpri[3*i+2] = to2pi(Xpri[3*i+2]);
						Xpos[3*i+2] = to2pi(Xpos[3*i+2]);
				}			
				update_pose();
		}
		
		
		void update_pose(){
				double e[3],v[3];
				int i,j,k,f,indi,ret;
				k=0;
				/**
				 * Lasketaan virhe mittauksen ja priorin välillä
				 * ja sitten päivitettään pose-vektorin varianssit CC ja Ce
				*/
				fprintf(stderr,"update_pose(1), N=%d ",N);
				for(j=0;j<N;j++) /* for each measurement */
				{
						f=frame[j];
						v[0]=base[3*j];
						v[1]=base[3*j+1];
						v[2]=to2pi(base[3*j+2]);
						e[0]=-v[0]+Xpri[3*f];
						e[1]=-v[1]+Xpri[3*f+1];
						e[2]=-v[2]+Xpri[3*f+2];
						e[2]=to2pi(e[2]);
						er[k]=e[0];
						k++;
						er[k]=e[1];
						k++;
						er[k]=e[2];
						k++;
						calc2D_init_Ce(Ce,P0+j*3,f,e);
						calc2D_init_CC(CC,P0+j*3,f,D);
				}
				fprintf(stderr,"update_pose(2) ");
				/**
				 * Lasketaan virhe odometrian ja priorin välillä
				 * ja päivitetään pose-vektorin varianssit CC ja Ce
				**/
				mcl::pose pos;
				mcl::pose new_pos;
				for(i=1;i<num_frames;i++){
				/// siirtymä error = (Xpri(i-1)+odo) - Xpri(i)  
						pos.set(Xpri[(i-1)*3],Xpri[(i-1)*3+1],Xpri[(i-1)*3+2]);
						new_pos = pos.integrateDifferential(mcl::pose(odo[(i-1)*3],odo[(i-1)*3+1],odo[(i-1)*3+2]));
						e[0] = Xpri[i*3] - new_pos.x;
						e[1] = Xpri[i*3+1] - new_pos.y;
						e[2] = Xpri[i*3+2] - new_pos.a;
						//e[0]=-odo[(i-1)*3]-Xpri[(i-1)*3]+Xpri[i*3];
						//e[1]=-odo[(i-1)*3+1]-Xpri[(i-1)*3+1]+Xpri[i*3+1];
						//e[2]=-odo[(i-1)*3+2]-Xpri[(i-1)*3+2]+Xpri[i*3+2];
						e[2]=to2pi(e[2]);
						/*calculate covariance matrix multiplications */
						calc2D_link_Ce(Ce,S,e,i);
						calc2D_link_CC(CC,S,D,i);
		
						er[k]=e[0];
						k++;
						er[k]=e[1];
						k++;
						er[k]=e[2];
						k++;
				}
				fprintf(stderr,"update_pose(3) ");
				/**
				 * Ratkaistaan g = inv(CC)Ce
				 * Levenberg-Marknad 
				**/
				int cnt=0;
				do
				{
						for(j=0;j<D*num_frames*3;j++)
								P[j]=CC[j];
						for(j=0;j<num_frames*3;j++)
								P[j*D]+=(lambda);
						ret=choldcb(L,P,num_frames*3,D);
						(lambda)*=12.;
						cnt ++;
						if((cnt+1)%100==0) fprintf(stderr,"Still here!\n");
				}
				while(ret<0);
				fprintf(stderr,"update_pose(4) ");
				cholslb(L,Ce,g,num_frames*3,D);
				for(j=0;j<num_frames*3;j++)Xpos[j]=Xpri[j]-g[j];
				fprintf(stderr,"update_pose(5) ");
		}
		
		
		
		void sfree(void *ptr){
				if(ptr) free(ptr);
		}
		
		
		double to2pi(double val) 
		{
				if(isnan(val) != 0){
						fprintf(stderr,"-- to2pi(%.3f) ",val);
						return 0;
				}
				if(val < -M_PI) while(val < -M_PI) val += 2.0*(double)M_PI;
				else if(val > M_PI)	while(val > M_PI) val -= 2.0*(double)M_PI;
				//fprintf(stderr,"out --");
				return val;
		}


		int choldcb(double *R,double *A,unsigned  int N, unsigned int p)
		{
				int i,j,k,l,lambda;
				double ir;
			
				for (j=0;j<N;j++)
				{
						lambda=j+p;
						if(lambda>N)
								lambda=N;
						lambda=lambda-j;

						for (i=0;i<lambda;i++)
								R[i+j*p]=A[i+j*p];

						k=j-p;
						if(k<0)
								k=0;	

						for (;k<j;k++)
						{
								l=k+p;	
								if(l>N)
										l=N;
								l=l-j;
								for (i=0;i<l;i++) R[i+j*p]=R[i+j*p]-R[i+(j-k)+k*p]*R[(j-k)+k*p];
						}
						if(R[j*p]<0)
								return 0;
    
						ir=1./sqrt(R[j*p]);
						for (i=0;i<lambda;i++) R[i+j*p]=R[i+j*p]*ir;
				}
				return 1;
		}

		void cholslb(double *A, double *b, double *x, int N, unsigned int p)
		{
				int i,k,stop;
				double sum;
    
				for (i=0;i<N;i++)
				{ 
						stop=i-p+1;
						if(stop<0)
								stop=0;
						for (sum=b[i],k=i-1;k>=stop;k--) sum -= A[(i-k)+k*p]*x[k];
						x[i]=sum/A[i*p]; /*BOOGIE*/
				}
				for (i=N-1;i>=0;i--)
				{
						stop=p;
						if(i+p>N)
								stop=N-i;
						for (sum=x[i],k=1;k<stop;k++) sum -= A[k+i*p]*x[i+k];
						x[i]=sum/A[i*p];/*BOOGIE*/
				}
		}
/**
		 * Laskee mittauksen kovarianssin
 */
		void calc2D_init_CC(
						double *CC,
		double *P,
		int frame,
		unsigned int N
											 )
		{	
				unsigned int i;
	
				for(i=0;i<3;i++)
						CC[(i+frame*3)*N]+=P[i];
	
		}
/**
		 * Päivittää mittauksen ristikovarianssimatriisia
 **/
		void calc2D_init_Ce(
						double *Ce,
		double *P,
		int frame,
		double *e
											 )
		{
				Ce[frame*3]+=P[0]*e[0];
				Ce[frame*3+1]+=P[1]*e[1];
				Ce[frame*3+2]+=P[2]*e[2];

		}

/**
		 * Päivittää odometriamittauksella kovarianssi matriisia
 */
		void calc2D_link_CC(
						double *CC,
		double *S,
		unsigned int N,
		unsigned int frame
											 )
		{	
				unsigned int base;
				base=frame*3-3;

/* e[0] 
				e[0]=X[3*frame]  -X[3*(frame-1)];
				e[1]=X[3*frame+1]-X[3*(frame-1)+1];
				e[2]=X[3*frame+2]-X[3*(frame-1)+2];
	
	*/
				/* CC[i*N+j-i]=JJ(i,j)*/
				CC[(base)*N]+=S[0];
				CC[(3+base)*N]+=S[0];
				CC[3+base*N]+=-S[0];
				/* e[1]*/ 	
				CC[(1+base)*N]+=S[1];
				CC[(4+base)*N]+=S[1];
				CC[3+(1+base)*N]+=-S[1];
	
	
				/* e[2]*/ 	
				CC[(2+base)*N]+=S[2];
				CC[(5+base)*N]+=S[2];
				CC[3+(2+base)*N]+=-S[2];

		}
/**
		 * Päivittää odometriamittauksella ristikovarianssi matriisia
 */
		void calc2D_link_Ce(
						double *Ce,
		double *S,
		double *e,
		unsigned int frame
											 )
		{
				unsigned int base;
				/* x_frame-x_frame-1 */
				base=frame*3-3; 

				Ce[base]-=e[0]*S[0];
				Ce[base+1]-=e[1]*S[1];
				Ce[base+2]-=e[2]*S[2];
				Ce[base+3]+=e[0]*S[0];
				Ce[base+4]+=e[1]*S[1];
				Ce[base+5]+=e[2]*S[2];

		}


};